
#include "Minesweeper.h"

#include <QGraphicsScene>
#include <QPainter>
#include <QStyleOption>
#include "CParams.h"
#include <math.h>

static const double Pi = 3.14159265358979323846264338327950288419717;
//static double TwoPi = 2.0 * Pi;
//vector<SVector2D> Minesweeper::sV_mines;
static double MaxTurnRate = CParams::dMaxTurnRate;

/*static qreal normalizeAngle(qreal angle)
 {
     while (angle < 0)
         angle += TwoPi;
     while (angle > TwoPi)
         angle -= TwoPi;
     return angle;
 }*/

Minesweeper::Minesweeper()
    : angle(0), speed(0), m_dFitness(0)
{
    rotate(qrand() % (360 * 16));
}

QRectF Minesweeper::boundingRect() const
{
    // qreal adjust = .5;
    // return QRectF(-18 - adjust, -22 - adjust,
    //                    36 + adjust, 60 + adjust);
    return QRect(-18,-22,44,36);
}

QPainterPath Minesweeper::shape() const
{
    QPainterPath path;
    path.addRect(-16, -20, 23, 20);
    return path;
}

void Minesweeper::paint(QPainter *painter, const QStyleOptionGraphicsItem *, QWidget *)
{
    pen = QPen(Qt::black);
    // Body
    painter->setPen(pen);
    painter->drawRect(-4, -5, 8, 15);

    // Eyes
    /*painter->setBrush(Qt::white);
     painter->drawEllipse(-10, -17, 8, 8);
     painter->drawEllipse(2, -17, 8, 8);*/

    // Nose
    // painter->setBrush(Qt::black);
    //painter->drawEllipse(QRectF(-2, -22, 4, 4));

    // Pupils
    // painter->drawEllipse(QRectF(-8.0 + MinesweeperEyeDirection, -17, 4, 4));
    // painter->drawEllipse(QRectF(4.0 + MinesweeperEyeDirection, -17, 4, 4));

    // Ears
    //painter->setBrush(scene()->collidingItems(mines).isEmpty() ? Qt::darkYellow : Qt::red);

    painter->drawRect(-9, -5, 4, 10);
    painter->drawRect(5, -5, 4, 10);

    // Tail
    /* QPainterPath path(QPointF(0, 20));
     path.cubicTo(-5, 22, -5, 22, 0, 25);
     path.cubicTo(5, 27, 5, 32, 0, 30);
     path.cubicTo(-5, 32, -5, 42, 0, 35);
     painter->setBrush(Qt::NoBrush);
     painter->drawPath(path);*/
}

/*void Minesweeper::advance(int step)
 {
     if (!step)
         return;
     update();
     // Don't move too far away
    /* QLineF lineToCenter(QPointF(0, 0), mapFromScene(0, 0));
     if (lineToCenter.length() > 150) {
         qreal angleToCenter = ::acos(lineToCenter.dx() / lineToCenter.length());
         if (lineToCenter.dy() < 0)
             angleToCenter = TwoPi - angleToCenter;
         angleToCenter = normalizeAngle((Pi - angleToCenter) + Pi / 2);

         if (angleToCenter < Pi && angleToCenter > Pi / 4) {
             // Rotate left
             angle += (angle < -Pi / 2) ? 0.25 : -0.25;
         } else if (angleToCenter >= Pi && angleToCenter < (Pi + Pi / 2 + Pi / 4)) {
             // Rotate right
             angle += (angle < Pi / 2) ? 0.25 : -0.25;
         }
     } else if (::sin(angle) < 0) {
         angle += 0.25;
     } else if (::sin(angle) > 0) {
         angle -= 0.25;
     }

     // Try not to crash with any other mice
     QList<QGraphicsItem *> dangerMice = scene()->items(QPolygonF()
                                                        << mapToScene(0, 0)
                                                        << mapToScene(-30, -50)
                                                        << mapToScene(30, -50));
     foreach (QGraphicsItem *item, dangerMice) {
         if (item == this)
             continue;

         QLineF lineToMinesweeper(QPointF(0, 0), mapFromItem(item, 0, 0));
         qreal angleToMinesweeper = ::acos(lineToMinesweeper.dx() / lineToMinesweeper.length());
         if (lineToMinesweeper.dy() < 0)
             angleToMinesweeper = TwoPi - angleToMinesweeper;
         angleToMinesweeper = normalizeAngle((Pi - angleToMinesweeper) + Pi / 2);

         if (angleToMinesweeper >= 0 && angleToMinesweeper < Pi / 2) {
             // Rotate right
             angle += 0.5;
         } else if (angleToMinesweeper <= TwoPi && angleToMinesweeper > (TwoPi - Pi / 2)) {
             // Rotate left
             angle -= 0.5;
         }
     }

     // Add some random movement
     if (dangerMice.size() > 1 && (qrand() % 10) == 0) {
         if (qrand() % 1)
             angle += (qrand() % 100) / 500.0;
         else
             angle -= (qrand() % 100) / 500.0;
     }

     speed += (-50 + qrand() % 100) / 100.0;

     qreal dx = ::sin(angle) * 10;
     MinesweeperEyeDirection = (qAbs(dx / 5) < 1) ? 0 : dx / 5;

     rotate(dx);
     setPos(mapToParent(0, -(3 + sin(speed) * 3)));
 }*/

     void Minesweeper::update()
     {
         // m_vPosition.x = pos().x();
         // m_vPosition.y = pos().y();

         //this will store all the inputs for the NN
         vector<double> inputs;

         //get vector to closest mine
         SVector2D vClosestMine = GetClosestMine(sV_mines);
         //SVector2D closestMine = GetCloseMine(sV_mines);

         //normalise it
         Vec2DNormalize(vClosestMine);

         //add in vector to closest mine
         inputs.push_back(vClosestMine.x);
         inputs.push_back(vClosestMine.y);

         //add in sweepers look at vector
         inputs.push_back(m_vLookAt.x);
         inputs.push_back(m_vLookAt.y);

         //update the brain and get feedback
         vector<double> output =  m_ItsBrain.Update(inputs);

         //make sure there were no errors in calculating the
         //output
         //	if (output.size() < CParams::iNumOutputs)
         //{
         //return false;
         //}
         int posNeg = 0;
         //assign the outputs to the sweepers left & right tracks
         m_lTrack = output[0];
         m_rTrack = output[1];
         //double turn = atan2(m_lTrack,m_rTrack);

         if(m_lTrack > m_rTrack)
             posNeg = 1;
         else
             posNeg = -1;
         //calculate steering forces
         double RotForce = m_lTrack - m_rTrack;

         //clamp rotation
         Clamp(RotForce, -.29999999, .299999999);

         m_dRotation += RotForce;
         angle = 0;




         m_dSpeed = (m_lTrack + m_rTrack);
         speed = m_dSpeed;

         //update Look At
         m_vLookAt.x = -sin(m_dRotation);
         m_vLookAt.y = cos(m_dRotation);
         //double turn = atan2(m_vLookAt.y,m_vLookAt.x);
         //rotate((fabs(m_vLookAt.x - m_vLookAt.y)*posNeg)*10);
         //update position
         m_vPosition += (m_vLookAt * m_dSpeed * 3);
         //double newY = -(2 + sin(m_dSpeed));
         setPos(m_vPosition.x,m_vPosition.y);
         double posX = pos().x();
         double posY = pos().y();

         if(posX > CParams::iWindowWidth)
         {
             setPos(0,posY);
             this->m_vPosition = SVector2D(0,posY);
         }
         if(posX < 0)
         {
             setPos(CParams::iWindowWidth, posY);
             this->m_vPosition = SVector2D(CParams::iWindowWidth,posY);
         }
         if(posY > CParams::iWindowHeight)
         {
             setPos(posX, 0);
             this->m_vPosition = SVector2D(posX,0);
         }
         if(posY < 0)
         {
             setPos(posX, CParams::iWindowHeight);
             this->m_vPosition = SVector2D(posX,CParams::iWindowHeight);
         }
     }

     SVector2D Minesweeper::GetClosestMine(vector<SVector2D> &v_mines)
     {
         double			closest_so_far = 99999;

         SVector2D		vClosestObject(0, 0);

         //cycle through mines to find closest
         for (int i=0; i<(int)v_mines.size(); i++)
         {
             double len_to_object = Vec2DLength(v_mines[i] - m_vPosition);

             if (len_to_object < closest_so_far)
             {
                 closest_so_far	= len_to_object;

                 vClosestObject	= m_vPosition - v_mines[i];

                 m_iClosestMine = i;
             }
         }

         return vClosestObject;
     }

     SVector2D Minesweeper::GetCloseMine(vector<SVector2D> &v_mines)
     {
         double			closest_so_far = 99999;

         SVector2D		vClosestObject(0, 0);

         //cycle through mines to find closest
         for (int i=0; i<(int)v_mines.size(); i++)
         {
             double len_to_object = Vec2DLength(v_mines[i] - m_vPosition);

             if (len_to_object < closest_so_far)
             {
                 closest_so_far	= len_to_object;

                 vClosestObject	= v_mines[i];

                 m_iClosestMine = i;
             }
         }

         return vClosestObject;
     }

     void Minesweeper::setMines(vector<SVector2D> v_mine)
     {
         sV_mines = v_mine;

     }

     void Minesweeper::increaseFitness(int num)
     {
         m_dFitness += num;
     }

     void Minesweeper::Reset()
     {
         double x = RandFloat() * CParams::iWindowWidth;
         double y = RandFloat() * CParams::iWindowHeight;
         this->setPos(x,y);
         this->m_vPosition = SVector2D(x,y);
         this->m_dFitness = 0;
         rotate(qrand() % (360 * 16));
     }

     void Minesweeper::setPosition(SVector2D p)
     {
         m_vPosition = p;
     }
     void Minesweeper::setPen(QPen p)
     {
         pen = p;
     }

     double Minesweeper::getFitness()
     {
         return this->m_dFitness;
     }

     vector<double> Minesweeper::getWeights()
     {
         return m_ItsBrain.GetWeights();
     }
